From b033f4849b3f4e789a641d8a517c0df7b95f66a7 Mon Sep 17 00:00:00 2001 From: robertl Date: Wed, 9 Aug 2006 03:09:46 +0000 Subject: [PATCH] Andy Armstrong adds support for version 4 of the WBT-200 (adds alt to tracks). Don't compute redundant course data in reader. --- reference/wbt-200.gpx | 518 ------------------------------------------ wbt-200.c | 476 +++++++++++++++++++++++++++++++------- 2 files changed, 392 insertions(+), 602 deletions(-) diff --git a/reference/wbt-200.gpx b/reference/wbt-200.gpx index 8b1f4a4ca..eb3f0cf4e 100644 --- a/reference/wbt-200.gpx +++ b/reference/wbt-200.gpx @@ -9,1298 +9,780 @@ xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/ - 12.265997 - 0.000000 WP0001 - 265.866974 - 0.000000 WP0002 - 301.257202 - 0.000000 WP0003 - 42.180206 - 0.000000 WP0004 - 30.913025 - 0.000000 WP0005 - 341.948425 - 0.000000 WP0006 - 263.101074 - 0.000000 WP0007 - 233.786987 - 0.000000 WP0008 - 217.187912 - 0.000000 WP0009 - 213.097519 - 0.000000 WP0010 - 210.878372 - 0.000000 WP0011 - 217.118881 - 0.000000 WP0012 - 217.958923 - 0.000000 WP0013 - 215.938080 - 0.000000 WP0014 - 217.554001 - 0.000000 WP0015 - 210.950439 - 0.000000 WP0016 - 198.590942 - 0.000000 WP0017 - 184.301193 - 0.000000 WP0018 - 190.879929 - 0.000000 WP0019 - 211.405655 - 0.000000 WP0020 - 215.783081 - 0.000000 WP0021 - 208.488708 - 0.000000 WP0022 - 199.536850 - 0.000000 WP0023 - 180.000000 - 0.000000 WP0024 - 192.201340 - 0.000000 WP0025 - 190.879929 - 0.000000 WP0026 - 190.879929 - 0.000000 WP0027 - 150.031418 - 0.000000 WP0028 - 106.121468 - 0.000000 WP0029 - 176.041763 - 0.000000 WP0030 - 260.558258 - 0.000000 WP0031 - 0.000000 - 0.000000 WP0032 - 158.972595 - 0.000000 WP0033 - 167.944855 - 0.000000 WP0034 - 150.031418 - 0.000000 WP0035 - 68.035133 - 0.000000 WP0036 - 14.799724 - 1.275627 WP0037 - 335.935822 - 1.063023 WP0038 - 343.458862 - 1.275627 WP0039 - 344.340790 - 1.275627 WP0040 - 4.152550 - 2.551255 WP0041 - 12.987873 - 1.275627 WP0042 - 317.683990 - 1.275627 WP0043 - 306.394348 - 1.275627 WP0044 - 329.722809 - 1.275627 WP0045 - 318.794800 - 1.275627 WP0046 - 317.018250 - 1.275627 WP0047 - 314.711304 - 1.275627 WP0048 - 322.087402 - 1.275627 WP0049 - 311.210266 - 1.275627 WP0050 - 317.634338 - 1.275627 WP0051 - 266.152252 - 0.000000 WP0052 - 149.760162 - 1.275627 WP0053 - 146.228149 - 1.275627 WP0054 - 137.398193 - 1.275627 WP0055 - 133.079147 - 1.275627 WP0056 - 129.032654 - 1.275627 WP0057 - 137.435822 - 1.275627 WP0058 - 138.555069 - 1.275627 WP0059 - 140.802185 - 1.275627 WP0060 - 143.721832 - 1.275627 WP0061 - 150.758606 - 1.275627 WP0062 - 176.012894 - 1.275627 WP0063 - 168.129211 - 1.275627 WP0064 - 159.790939 - 1.275627 WP0065 - 171.529037 - 1.275627 WP0066 - 148.770325 - 1.275627 WP0067 - 132.740677 - 0.000000 WP0068 - 187.189056 - 0.000000 WP0069 - 47.487198 - 1.275627 WP0070 - 167.396683 - 1.275627 WP0071 - 53.021553 - 0.000000 WP0072 - 36.382450 - 0.000000 WP0073 - 176.592499 - 0.000000 WP0074 - 341.019470 - 1.275627 WP0075 - 164.926804 - 0.000000 WP0076 - 213.118225 - 10.205019 WP0077 - 229.895782 - 3.826882 WP0078 - 200.457092 - 2.551255 WP0079 - 200.398605 - 5.102509 WP0080 - 119.491547 - 1.275627 WP0081 - 30.855570 - 2.551255 WP0082 - 27.292553 - 1.275627 WP0083 - 28.641382 - 1.275627 WP0084 - 27.930870 - 1.275627 WP0085 - 29.115019 - 1.275627 WP0086 - 27.866358 - 1.275627 WP0087 - 24.874218 - 1.275627 WP0088 - 27.565464 - 1.063023 WP0089 - 25.332216 - 0.000000 WP0090 - 26.914623 - 0.000000 WP0091 - 25.907835 - 0.000000 WP0092 - 26.904547 - 0.000000 WP0093 - 29.328938 - 0.000000 WP0094 - 29.442163 - 0.000000 WP0095 - 28.902466 - 0.000000 WP0096 - 29.034962 - 0.000000 WP0097 - 28.407316 - 0.000000 WP0098 - 66.844353 - 3.826882 WP0099 - 171.949890 - 6.378137 WP0100 - 192.653625 - 7.653764 WP0101 - 189.704391 - 5.102509 WP0102 - 168.316635 - 5.102509 WP0103 - 178.446686 - 1.275627 WP0104 - 133.201309 - 2.551255 WP0105 - 5.955359 - 0.000000 WP0106 - 222.597931 - 1.275627 WP0107 - 169.542999 - 3.826882 WP0108 - 140.863525 - 2.551255 WP0109 - 134.634933 - 2.551255 WP0110 - 315.697510 - 2.126046 WP0111 - 49.662453 - 2.126046 WP0112 - 329.456329 - 6.378137 WP0113 - 14.441216 - 3.826882 WP0114 - 21.177444 - 6.378137 WP0115 - 349.242554 - 5.102509 WP0116 - 333.408875 - 3.826882 WP0117 - 323.969238 - 3.826882 WP0118 - 300.761627 - 2.551255 WP0119 - 23.503998 - 1.275627 WP0120 - 355.272217 - 0.000000 WP0121 - 1.028101 - 2.551255 WP0122 - 278.163086 - 1.063023 WP0123 - 38.421696 - 0.000000 WP0124 - 21.123373 - 0.000000 WP0125 - 318.335754 - 0.000000 WP0126 - 55.058521 - 0.000000 WP0127 - 318.108948 - 0.000000 WP0128 - 314.344055 - 1.275627 WP0129 - 58.753159 - 1.275627 WP0130 - 80.213234 - 0.000000 WP0131 - 54.398392 - 1.275627 WP0132 - 91.628487 - 0.000000 WP0133 - 256.810364 - 0.000000 WP0134 - 56.125755 - 0.000000 WP0135 - 52.938625 - 0.000000 WP0136 - 44.161476 - 0.000000 WP0137 - 295.567017 - 0.000000 WP0138 - 45.710194 - 0.000000 WP0139 - 17.311560 - 0.000000 WP0140 - 15.516356 - 0.000000 WP0141 - 46.959927 - 0.000000 WP0142 - 51.550701 - 1.275627 WP0143 - 310.929291 - 0.000000 WP0144 - 81.424187 - 0.000000 WP0145 - 152.140488 - 0.000000 WP0146 - 77.768639 - 0.000000 WP0147 - 82.401405 - 0.000000 WP0148 - 61.328655 - 0.000000 WP0149 - 50.326954 - 0.000000 WP0150 - 211.192841 - 0.000000 WP0151 - 99.649384 - 0.000000 WP0152 - 90.000000 - 0.000000 WP0153 - 205.665054 - 0.000000 WP0154 - 98.395973 - 0.000000 WP0155 - 72.691803 - 0.000000 WP0156 - 69.942642 - 0.000000 WP0157 - 68.092773 - 0.000000 WP0158 - 180.000000 - 0.000000 WP0159 - 213.384644 - 0.000000 WP0160 - 233.378494 - 0.000000 WP0161 - 230.063202 - 0.000000 WP0162 - 229.070709 - 0.000000 WP0163 - 227.233322 - 0.000000 WP0164 - 235.251038 - 0.000000 WP0165 - 229.070709 - 0.000000 WP0166 - 239.968582 - 0.000000 WP0167 - 239.074066 - 0.000000 WP0168 - 239.474564 - 0.000000 WP0169 - 235.251038 - 0.000000 WP0170 - 231.751541 - 0.000000 WP0171 - 220.857513 - 0.000000 WP0172 - 217.554047 - 0.000000 WP0173 - 229.070709 - 0.000000 WP0174 - 261.776520 - 0.000000 WP0175 - 280.345551 - 0.000000 WP0176 - 255.061081 - 0.000000 WP0177 - 258.684265 - 0.000000 WP0178 - 280.906860 - 0.000000 WP0179 - 270.000000 - 0.000000 WP0180 - 0.000000 - 0.000000 WP0181 - 29.968607 - 0.000000 WP0182 - 34.998993 - 0.000000 WP0183 - 37.958961 - 0.000000 WP0184 - 27.290253 - 0.000000 WP0185 - 20.546144 - 0.000000 WP0186 - 357.936035 - 0.000000 WP0187 - 298.455536 - 0.000000 WP0188 - 313.934113 - 0.000000 WP0189 - 343.917206 - 0.000000 WP0190 - 19.818523 - 0.000000 WP0191 - 26.772879 - 0.000000 WP0192 - 38.409218 - 0.000000 WP0193 - 43.137356 - 0.000000 WP0194 - 50.781590 - 0.000000 WP0195 - 49.070702 - 0.000000 WP0196 - 52.375839 - 0.000000 WP0197 - 45.259075 - 0.000000 WP0198 - 31.405672 - 0.000000 WP0199 - 29.968596 - 0.000000 WP0200 - 34.681049 - 0.000000 WP0201 - 41.414547 - 3.826882 WP0202 - 44.116512 - 6.378137 WP0203 - 44.546139 - 6.378137 WP0204 - 38.584755 - 5.102509 WP0205 - 38.722885 - 3.826882 WP0206 - 47.336823 - 8.929392 WP0207 - 211.194199 - 8.929392 WP0208 - 220.849304 - 10.205019 WP0209 - 222.281082 - 7.653764 WP0210 - 214.937927 - 6.378137 WP0211 - 92.128235 - 1.275627 WP0212 - 206.767715 - 5.102509 WP0213 - 242.294983 - 3.826882 WP0214 - 254.594803 - 3.826882 WP0215 - 295.212555 - 0.000000 WP0216 - 38.875465 - 7.653764 WP0217 - 40.146595 - 10.205019 WP0218 - 224.759964 - 8.929392 WP0219 - 225.272110 - 2.551255 WP0220 - 39.239895 - 2.551255 WP0221 - 87.030205 - 0.000000 WP0222 - 46.297512 - 8.929392 WP0223 - 231.639389 - 0.000000 WP0224 - 251.805908 - 0.000000 WP0225 - 6.045611 - 0.000000 WP0226 - 278.639679 - 1.275627 WP0227 - 353.288483 - 0.000000 WP0228 - 41.914322 - 7.653764 WP0229 - 48.300850 - 3.826882 WP0230 - 48.979435 - 6.378137 WP0231 - 40.241310 - 6.378137 WP0232 - 262.283997 - 2.551255 WP0233 - 226.113998 - 15.307528 WP0234 - 219.944962 - 7.653764 WP0235 - 234.476700 - 2.551255 WP0236 - 225.160934 - 7.653764 WP0237 - 208.157425 - 3.826882 WP0238 - 48.920593 - 1.275627 WP0239 - 215.755188 - 3.826882 WP0240 - 47.400951 - 11.480646 WP0241 - 47.977039 - 12.756274 WP0242 - 47.508724 - 17.858784 WP0243 - 233.257462 - 19.134411 WP0244 - 211.781097 - 7.175404 WP0245 - 15.920190 - 2.733487 WP0246 - 186.826736 - 0.911162 WP0247 - 70.870804 - 1.962504 WP0248 - 330.239197 - 1.275627 WP0249 - 62.098572 - 14.031901 WP0250 - 257.446716 - 7.653764 WP0251 - 42.669178 - 3.826882 WP0252 - 328.710785 - 2.551255 WP0253 - 49.378994 - 11.480646 WP0254 - 196.559738 - 2.733487 WP0255 - 230.720337 - 3.029615 WP0256 - 58.682175 - 22.961292 WP0257 - 239.238800 - 1.075830 WP0258 - 22.855204 - 0.135705 WP0259 diff --git a/wbt-200.c b/wbt-200.c index 405506447..e04811d45 100644 --- a/wbt-200.c +++ b/wbt-200.c @@ -1,21 +1,21 @@ /* * Serial download of track data from a Wintec WBT-200. - * + * * Copyright (C) 2006 Andy Armstrong - * - * This program is free software; you can redistribute it and/or modify it + * + * This program is free software; you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the * Free Software Foundation; either version 2 of the License, or (at your * option) any later version. - * + * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place - Suite 330, Boston, MA 02111 USA + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111 USA */ #include "defs.h" @@ -23,9 +23,44 @@ #include "grtcirc.h" #include +#define MYNAME "WBT-100/200" +#define NL "\x0D\x0A" + #define BAUD 9600 #define TIMEOUT 1500 +#define RECLEN_V1 12 +#define RECLEN_V2 16 + +/* Used to sanity check data - from + * http://hypertextbook.com/facts/2001/DanaWollman.shtml + * The MAXALT check doesn't need to be enabled unless there's + * a format with larger records than V2. + */ +/*#define MAXALT 120000*/ + +#define _MAX(a, b) ((a) > (b) ? (a) : (b)) +#define RECLEN_MAX _MAX(RECLEN_V1, RECLEN_V2) + +/* The formats here must be in ascending record length order so that + * each format identification attempt can read more data from the + * device if necessary. If that proves to be a bad order to try the + * heuristics the format matching code will have to be rejigged. + */ +static struct { + size_t reclen; +} fmt_version[] = { + { RECLEN_V1 }, + { RECLEN_V2 }, + { 0 } +}; + +/* Number of lines to skip while waiting for an ACK from a command. I've seen + * conversations with up to 30 lines of cruft before the response so 50 isn't + * too crazy. + */ +#define RETRIES 50 + /* A conversation looks like this @@ -36,33 +71,48 @@ >> $PFST,READLOGGER << $PFST,READLOGGER,*17 << 0xFFFF, , 0xFFFF - << (length + 1) * 12 bytes of data + << (length + 1) * 12 or 16 bytes of data << ==== >> $PFST,NORMAL << $PFST,NORMAL,*02 */ -/*static gpsdevh *fd;*/ static void *fd; static FILE *fl; static char *port; static char *erase; -#define MYNAME "WBT-100/200" -#define NL "\x0D\x0A" +struct buf_chunk { + struct buf_chunk *next; + size_t size; + size_t used; + /* data follows in memory */ +}; -struct read_state { - route_head *route_head; - double plat, plon; /* previous point */ - time_t ptim; - unsigned wpn; +#define buf_CHUNK_DATA(c) \ + ((void *) ((struct buf_chunk *) (c) + 1)) + +#define buf_CHUNK_PTR(c, offset) \ + ((void *) ((char *) buf_CHUNK_DATA(c) + (offset))) + +struct buf_head { + struct buf_chunk *head; + struct buf_chunk *tail; + size_t alloc; + size_t used; + /* read position */ + struct buf_chunk *current; + unsigned long offset; }; -/* Number of lines to skip while waiting for an ACK from a command. I've seen - * conversations with up to 30 lines of cruft before the response so 50 isn't - * too crazy. - */ -#define RETRIES 50 +struct read_state { + route_head *route_head; + double plat, plon; /* previous point */ + time_t ptim; + unsigned wpn; + + struct buf_head data; +}; static void db(int l, const char *msg, ...) { va_list ap; @@ -73,6 +123,99 @@ static void db(int l, const char *msg, ...) { va_end(ap); } +/* Growable buffer support. TODO: Put this in a separate file and + * tidy up its interface. + */ + +static void buf_init(struct buf_head *h, size_t alloc) { + h->head = NULL; + h->tail = NULL; + h->alloc = alloc; + h->used = 0; +} + +static void buf_empty(struct buf_head *h) { + struct buf_chunk *chunk, *next; + for (chunk = h->head; chunk; chunk = next) { + next = chunk->next; + xfree(chunk); + } + h->head = NULL; + h->tail = NULL; + h->used = 0; +} + +static void buf_rewind(struct buf_head *h) { + h->current = h->head; + h->offset = 0; +} + +static size_t buf_read(struct buf_head *h, void *data, size_t len) { + char *bp = data; + + while (len != 0 && h->current != NULL) { + size_t avail = h->current->used - h->offset; + if (avail > len) { avail = len; } + + memcpy(bp, buf_CHUNK_PTR(h->current, h->offset), avail); + h->offset += avail; + bp += avail; + len -= avail; + + if (h->offset == h->current->used) { + h->current = h->current->next; + h->offset = 0; + } + } + + return bp - (char *) data; +} + +static void buf_extend(struct buf_head *h, size_t amt) { + struct buf_chunk *c; + size_t sz = amt + sizeof(struct buf_chunk); + if (c = xmalloc(sz), NULL == c) { + fatal(MYNAME ": Can't allocate %lu bytes for buffer", (unsigned long) sz); + } + + c->next = NULL; + c->size = amt; + c->used = 0; + + if (NULL == h->head) { + h->head = c; + } else { + h->tail->next = c; + } + + h->tail = c; +} + +static void buf_write(struct buf_head *h, const void *data, size_t len) { + size_t avail; + const char *bp = data; + + h->used += len; + + if (NULL == h->tail) { + buf_extend(h, h->alloc); + } + + for (;;) { + avail = h->tail->size - h->tail->used; + if (avail > len) { avail = len; } + + memcpy((char *) buf_CHUNK_PTR(h->tail, h->tail->used), bp, avail); + h->tail->used += avail; + bp += avail; + len -= avail; + if (len == 0) { + break; + } + buf_extend(h, h->alloc); + } +} + static void rd_drain() { if (gbser_flush(fd)) { fatal(MYNAME ": Comm error\n"); @@ -139,12 +282,11 @@ static int starts_with(const char *buf, const char *pat) { /* Send a command then wait for a line starting with the command string * to be returned. */ - static void do_cmd(const char *cmd, const char *expect, char *buf, int len) { int try; rd_drain(); - wr_cmd(cmd); + wr_cmd(cmd); wr_cmd(NL); db(2, "Cmd: %s\n", cmd); @@ -165,32 +307,69 @@ static void do_cmd(const char *cmd, const char *expect, char *buf, int len) { fatal(MYNAME ": Bad response from unit\n"); } +/* Issue a command that expects the same string to be echoed + * back as an ACK + */ static void do_simple(const char *cmd, char *buf, int len) { do_cmd(cmd, cmd, buf, len); } -static void data_chunk(struct read_state *st, const void *buf) { - char wp_name[20]; - gbuint32 tim; - double lat, lon; - struct tm t; - time_t rtim; - waypoint *wpt = NULL; - const char *bp = buf; - +/* Decompose binary date into discreet fields */ +#define _SPLIT_DATE(tim) \ + int sec = (((tim) >> 0) & 0x3F); \ + int min = (((tim) >> 6) & 0x3F); \ + int hour = (((tim) >> 12) & 0x1F); \ + int mday = (((tim) >> 17) & 0x1F); \ + int mon = (((tim) >> 22) & 0x0F); \ + int year = (((tim) >> 26) & 0x3F); + +static time_t decode_date(gbuint32 tim) { + _SPLIT_DATE(tim) + struct tm t; + + t.tm_sec = sec; + t.tm_min = min; + t.tm_hour = hour; + t.tm_mday = mday; + t.tm_mon = mon - 1; + t.tm_year = year + 100; + + return mkgmtime(&t); +} + +static int check_date(gbuint32 tim) { + _SPLIT_DATE(tim) + + /* Sanity check the date. We don't allow years prior to 2004 because zero in + * those bits will usually indicate that we have an altitude instead of a + * date (i.e. that the data is the new format that uses 16 byte records). + */ + return sec < 60 && min < 60 && hour < 24 && + mday > 0 && mday <= 31 && mon > 0 && mon <= 12 && year >= 4; +} + +static int data_chunk(struct read_state *st, const void *buf, int fmt) { + char wp_name[20]; + gbuint32 tim; + double lat, lon, alt; + time_t rtim; + waypoint *wpt = NULL; + const char *bp = buf; + size_t buf_used = fmt_version[fmt].reclen; + tim = le_read32(bp + 0); - + lat = (double) ((gbint32) le_read32(bp + 4)) / 10000000; lon = (double) ((gbint32) le_read32(bp + 8)) / 10000000; - - t.tm_sec = ((tim >> 0) & 0x3F); - t.tm_min = ((tim >> 6) & 0x3F); - t.tm_hour = ((tim >> 12) & 0x1F); - t.tm_mday = ((tim >> 17) & 0x1F); - t.tm_mon = ((tim >> 22) & 0x0F) - 1; - t.tm_year = ((tim >> 26) & 0x3F) + 100; - - rtim = mkgmtime(&t); + + /* Handle extra fields in longer records here. */ + if (buf_used >= 16) { + alt = (double) le_read32(bp + 12) / 10; + } else { + alt = unknown_alt; + } + + rtim = decode_date(tim); if (lat >= 100) { /* Start new track in the northern hemisphere */ @@ -202,29 +381,21 @@ static void data_chunk(struct read_state *st, const void *buf) { lat += 100; st->route_head = NULL; } else { - double speed, gcd, dtim, rtm; + /* TODO: Should this code execute for /every/ waypoint - even the first in + * a track? Presumably it should because the first point looks as valid as + * any other. + */ + wpt = waypt_new(); - + wpt->latitude = lat;; wpt->longitude = lon; + wpt->altitude = alt; wpt->creation_time = rtim; wpt->centiseconds = 0; - + sprintf(wp_name, "WP%04d", ++st->wpn); wpt->shortname = xstrdup(wp_name); - - /* Broken down to make it easier to find the source of rounding errors */ - gcd = gcdist(RAD(st->plat), RAD(st->plon), RAD(lat), RAD(lon)); - gcd = (double) ((long) (gcd * 1000000 + 0.5)) / 1000000; - dtim = rtim - st->ptim; - rtm = radtometers(gcd); - speed = rtm / dtim; - - wpt->speed = speed; - wpt->course = heading_true_degrees(RAD(st->plat), RAD(st->plon), - RAD(lat), RAD(lon)); - wpt->pdop = 0; - wpt->fix = fix_unknown; if (NULL == st->route_head) { db(1, "New Track\n"); @@ -234,24 +405,127 @@ static void data_chunk(struct read_state *st, const void *buf) { track_add_wpt(st->route_head, wpt); } - + st->ptim = rtim; st->plat = lat; st->plon = lon; + + return 1; +} + +/* Return true iff the data appears valid with the specified record length */ +static int is_valid(struct buf_head *h, int fmt) { + char buf[RECLEN_MAX]; + size_t reclen = fmt_version[fmt].reclen; + + buf_rewind(h); + + db(2, "Checking %lu bytes of data against format %d\n", h->used, fmt); + + for (;;) { + size_t got = buf_read(h, buf, reclen); + gbuint32 tim; + /* Don't mind odd bytes at the end - we may + * be examining an incomplete dataset. + */ + if (got != reclen) { + break; + } + + tim = le_read32(buf + 0); + if (!check_date(tim)) { + return 0; + } + + if (reclen > 12) { +#ifdef MAXALT + gbuint32 alt = le_read32(buf + 12); + if (alt > MAXALT * 10) { + return 0; + } +#endif + } + } + + return 1; +} + +static void process_data(struct read_state *pst, int fmt) { + char buf[RECLEN_MAX]; + size_t reclen = fmt_version[fmt].reclen; + + buf_rewind(&pst->data); + + db(2, "Processing %lu bytes of data using format %d\n", pst->data.used, fmt); + + for (;;) { + size_t got = buf_read(&pst->data, buf, reclen); + if (got != reclen) { + break; + } + data_chunk(pst, buf, fmt); + } +} + +static void state_init(struct read_state *pst) { + pst->route_head = NULL; + pst->wpn = 0; + + buf_init(&pst->data, RECLEN_V1 * RECLEN_V2); +} + +static void state_empty(struct read_state *pst) { + buf_empty(&pst->data); + state_init(pst); } static void file_read(void) { - char buf[12]; - int rc; + char buf[512]; + size_t rc; struct read_state st; - - st.route_head = NULL; - st.wpn = 0; - - rc = fread(buf, sizeof(buf), 1, fl); - while (rc == 1) { - data_chunk(&st, buf); - rc = fread(buf, sizeof(buf), 1, fl); + int fmt; + + state_init(&st); + + /* Read the whole file into the buffer */ + rc = fread(buf, 1, sizeof(buf), fl); + while (rc != 0) { + buf_write(&st.data, buf, rc); + rc = fread(buf, 1, sizeof(buf), fl); + } + + if (!feof(fl)) { + fatal(MYNAME ": Read error"); + } + + /* Try to guess the data format */ + for (fmt = 0; fmt_version[fmt].reclen != 0; fmt++) { + size_t reclen = fmt_version[fmt].reclen; + if ((st.data.used % reclen) == 0 && is_valid(&st.data, fmt)) { + break; + } + } + + if (fmt_version[fmt].reclen == 0) { + fatal(MYNAME ": Can't autodetect data format"); + } + + process_data(&st, fmt); + + state_empty(&st); +} + +static void want_bytes(struct buf_head *h, size_t len) { + char buf[512]; + + db(3, "Reading %lu bytes from device\n", (unsigned long) len); + + while (len > 0) { + size_t want = sizeof(buf); + if (want > len) { want = len; } + rd_buf(buf, want); + buf_write(h, buf, want); + len -= want; } } @@ -261,31 +535,64 @@ static void data_read(void) { * long lines returning only the first N characters */ char line_buf[100]; - int count, d; + int fmt; + unsigned long count; struct read_state st; - - st.route_head = NULL; - st.wpn = 0; + state_init(&st); + + /* We could potentially parse the version string to find out which + * data format to use - but it's not clear how the version string + * will increment in the future - so just now it's more future- + * proof to rely on analysing the data. We need to be able to do + * that with files anyway - because they're not versioned. + */ do_simple("$PFST,FIRMWAREVERSION", line_buf, sizeof(line_buf)); + do_simple("$PFST,NORMAL", line_buf, sizeof(line_buf)); do_simple("$PFST,READLOGGER", line_buf, sizeof(line_buf)); - + /* Now we're into binary mode */ rd_buf(line_buf, 6); /* six byte header */ count = le_read16(line_buf + 2) + 1; if (count == 0x10000) { count = 0; } - - db(1, "Reading %d data\n", count); - for (d = 0; d < count; d++) { - rd_buf(line_buf, 12); /* twelve byte record */ - data_chunk(&st, line_buf); + + db(3, "%lu points available\n", count); + + /* Loop through the known formats requesting more data from the + * device each time. When the device contains only a single + * point the first format will get a false positive - so we'll + * lose the altitude data. + */ + for (fmt = 0; fmt_version[fmt].reclen != 0; fmt++) { + size_t reclen = fmt_version[fmt].reclen; + size_t want = reclen * count; + + if (want < st.data.used) { + fatal(MYNAME ": Internal error: formats not ordered in ascending size order"); + } + + db(3, "Want %lu bytes of data\n", (unsigned long) want); + + /* Top up the buffer */ + want_bytes(&st.data, want - st.data.used); + + /* And see if it's valid */ + if (is_valid(&st.data, fmt)) { + break; + } + } + + if (fmt_version[fmt].reclen == 0) { + fatal(MYNAME ": Can't autodetect data format"); } - + + process_data(&st, fmt); + /* Erase data? */ - + if (*erase != '0') { int f; db(1, "Erasing data\n"); @@ -301,11 +608,12 @@ static void data_read(void) { } do_simple("$PFST,NORMAL", line_buf, sizeof(line_buf)); - + + state_empty(&st); } static arglist_t wbt_sargs[] = { - { "erase", &erase, "Erase device data after download", + { "erase", &erase, "Erase device data after download", "0", ARGTYPE_BOOL, ARG_NOMINMAX }, ARG_TERMINATOR }; -- 2.30.2